#include "Rabbit.h"
#include "Chassis.h"
#include "Remote.h"
#include "FastMathFunctions.h"

/**
 * UART1 SBUS
 * UART6 ACTION
 * UART7
 * UART8 PC
 * CAN1 DJ
 * CAN1 RMDS
 * TIM7 MM
 */

/* 毫秒定时器 */
void MM_TIM_Callback(void)
{
    static uint16_t count;
    if (count++ > 20)
    {
        count = 0;

        // UART_Print("%.3f %.3f %.3f\n", Action.pos_x, Action.pos_y, Action.yaw);
        // UART_Print("%.3f %.3f %.3f\n", chassis.PID_X.x1, chassis.PID_Y.x1, chassis.PID_Angle.x1);
        // UART_Print("out:%f\n", chassis.PID_Y.out);
        // UART_Print("set:%f angle:%f\n", DEG(chassis.PID_Angle.set), Action.yaw);
        // UART_Print("angle:%.3f out:%.3f iout:%.3f dout:%.3f\n", Action.yaw, chassis.PID_Angle.out, chassis.PID_Angle.Iout, chassis.PID_Angle.Dout);
        // UART_Print("%.3f %.3f %.3f %.3f\n", chassis.Motors_Speed[0], chassis.Motors_Speed[1], chassis.Motors_Speed[2], chassis.Motors_Speed[3]);
        // UART_Print("pout:%.3f iout:%.3f dout:%.3f\n", chassis.PID_X.Pout, chassis.PID_X.Iout, chassis.PID_X.Dout);
        // UART_Print("setY:%.3f Y:%.3f\n", chassis.PID_Y.x1, Action.pos_y);
        // UART_Print("setX:%.3f X:%.3f\n", chassis.PID_X.x1, Action.pos_x);
        UART_Print("dx:%.3f, dy:%.3f, da:%.3f\n", Action.pos_x - chassis.PID_X.x1, Action.pos_y - chassis.PID_Y.x1, Action.yaw - chassis.PID_Angle.x1);
        // UART_Print("setY:%f setX:%f Y:%f X:%f\n", chassis.PID_Y.x1, chassis.PID_X.x1, Action.pos_y, Action.pos_x);
        // UART_Print("x:%f y:%f\n", Action.pos_x - OFFSET * sinf(-Action.yaw), Action.pos_y - OFFSET * (cosf(Action.yaw) - 1));
        // UART_Print("x:%f Ax:%f\n", Action.pos_x - OFFSET * sin(-Action.yaw), Action.pos_x);
        // UART_Print("%f\n", OFFSET * sin(-Action.yaw * PI / 180));
    }
}

/* 兔子初始化 */
inline void Rabbit_Init(void)
{
    AttachInterrupt_TIM(&htim7, MM_TIM_Callback);
    CAN_FilterInit(); // CAN过滤配置
    Chassis_Init();   // 底盘初始化
    Remote_Init();    // 遥控初始化

    /************* 初始化完成 ***********/
    UART_Print("Initialization complete\n");
    HAL_TIM_Base_Start_IT(&htim7);

    // HAL_GPIO_WritePin(POWER1_GPIO_Port, POWER1_Pin, GPIO_PIN_SET);
    // HAL_GPIO_WritePin(POWER2_GPIO_Port, POWER2_Pin, GPIO_PIN_SET);
    // HAL_GPIO_WritePin(POWER3_GPIO_Port, POWER3_Pin, GPIO_PIN_SET);
    // HAL_GPIO_WritePin(POWER4_GPIO_Port, POWER4_Pin, GPIO_PIN_SET);
    // Chassis_SetVel(100, 0, 0);
    Chassis_SetPos(0, 0, RAD(0));
    // Chassis_SetPos(0, 5000, 0);
}

/* 兔子运行循环 */
inline void Rabbit_Run(void)
{
    /* 底盘运行 */
    Chassis_Run();
}
